#include "ControlSurfaceHandler.h"

uint8 u8aileron_position = 128;
uint8 u8rudder_position = 128;
uint8 u8elevator_position = 128;

uint8 u8csurfaceCalibrationBackup[18];

extern uint8 u8surface_config;
extern uint8 u8config_feedback3;

uint16 u16aileron_temp;
uint16 u16rudder_temp;
uint16 u16elevator_temp;

#pragma CONST_SEG CONSTANTS
volatile const uint16 u16aileron_calibrated_min @0x00004000 = CSURFACE_MIN_VALUE;
volatile const uint16 u16aileron_calibrated_center @0x00004002 = CSURFACE_CENTER_VALUE;
volatile const uint16 u16aileron_calibrated_max @0x00004004 = CSURFACE_MAX_VALUE;

volatile const uint16 u16rudder_calibrated_min @0x00004006 = CSURFACE_MIN_VALUE;
volatile const uint16 u16rudder_calibrated_center @0x00004008 = CSURFACE_CENTER_VALUE;
volatile const uint16 u16rudder_calibrated_max @0x0000400A = CSURFACE_MAX_VALUE;

volatile const uint16 u16elevator_calibrated_min @0x0000400C = CSURFACE_MIN_VALUE;
volatile const uint16 u16elevator_calibrated_center @0x0000400E = CSURFACE_CENTER_VALUE;
volatile const uint16 u16elevator_calibrated_max @0x00004010 = CSURFACE_MAX_VALUE;

void vfnControlSurface_ConfigurePWM (void)
{                              
    vfnTPM_PrescalerSelect (CONTROL_SURFACE_TIMER, TPM_PRESCALER_64);
                       
    vfnTPM_ConfigureEAPWM (CONTROL_SURFACE_TIMER, AILERON_PWM_CHAN, TPM_HIGH_TRUE);
    vfnTPM_ConfigureEAPWM (CONTROL_SURFACE_TIMER, RUDDER_PWM_CHAN, TPM_HIGH_TRUE);
    vfnTPM_ConfigureEAPWM (CONTROL_SURFACE_TIMER, ELEVATOR_PWM_CHAN, TPM_HIGH_TRUE);
    
    vfnTPM_SetChannelValue (CONTROL_SURFACE_TIMER, AILERON_PWM_CHAN, AILERON_PWM_DEFAULT);
    vfnTPM_SetChannelValue (CONTROL_SURFACE_TIMER, RUDDER_PWM_CHAN, RUDDER_PWM_DEFAULT);
    vfnTPM_SetChannelValue (CONTROL_SURFACE_TIMER, ELEVATOR_PWM_CHAN, ELEVATOR_PWM_DEFAULT);
    
    vfnTPM_SetModulo (CONTROL_SURFACE_TIMER, 5000);    // 20ms period.
    vfnTPM_ClockSelect (CONTROL_SURFACE_TIMER, TPM_BUS_CLOCK);
}

void vfnControlSurface_Disable (void)
{
    vfnTPM_SetChannelValue (CONTROL_SURFACE_TIMER, AILERON_PWM_CHAN, AILERON_PWM_DEFAULT);
    vfnTPM_SetChannelValue (CONTROL_SURFACE_TIMER, RUDDER_PWM_CHAN, RUDDER_PWM_DEFAULT);
    vfnTPM_SetChannelValue (CONTROL_SURFACE_TIMER, ELEVATOR_PWM_CHAN, ELEVATOR_PWM_DEFAULT);    
}

void vfnControlSurface_Adjust (void)
{  
    if (u8aileron_position < 128)
    {
        u16aileron_temp = u16aileron_calibrated_center - u16aileron_calibrated_min;
        u16aileron_temp *= (128 - u8aileron_position);
        u16aileron_temp /= 97;
        u16aileron_temp = u16aileron_calibrated_center - u16aileron_temp;    
    }
    else
    {
        u16aileron_temp = u16aileron_calibrated_max - u16aileron_calibrated_center;
        u16aileron_temp *= (u8aileron_position - 128);
        u16aileron_temp /= 97;
        u16aileron_temp = u16aileron_calibrated_center + u16aileron_temp;   
    }
    
    if (u8rudder_position < 128)
    {
        u16rudder_temp = u16rudder_calibrated_center - u16rudder_calibrated_min;
        u16rudder_temp *= (128 - u8rudder_position);
        u16rudder_temp /= 97; 
        u16rudder_temp = u16rudder_calibrated_center - u16rudder_temp;
    }
    else
    {
        u16rudder_temp = u16rudder_calibrated_max - u16rudder_calibrated_center; 
        u16rudder_temp *= (u8rudder_position - 128);
        u16rudder_temp /= 97; 
        u16rudder_temp = u16rudder_calibrated_center + u16rudder_temp;
    }
    
    if (u8elevator_position < 128)
    {
        u16elevator_temp = u16elevator_calibrated_center - u16elevator_calibrated_min;
        u16elevator_temp *= (128 - u8elevator_position);
        u16elevator_temp /= 97; 
        u16elevator_temp = u16elevator_calibrated_center - u16elevator_temp;
    }
    else
    {
        u16elevator_temp = u16elevator_calibrated_max - u16elevator_calibrated_center; 
        u16elevator_temp *= (u8elevator_position - 128);
        u16elevator_temp /= 97;
        u16elevator_temp = u16elevator_calibrated_center + u16elevator_temp;
         
    }
    
    vfnTPM_SetChannelValue (CONTROL_SURFACE_TIMER, AILERON_PWM_CHAN, u16aileron_temp);
    vfnTPM_SetChannelValue (CONTROL_SURFACE_TIMER, RUDDER_PWM_CHAN, u16rudder_temp);
    vfnTPM_SetChannelValue (CONTROL_SURFACE_TIMER, ELEVATOR_PWM_CHAN, u16elevator_temp);
       
}

uint8 u8Csurface_SaveCalValue (uint8 u8selection)
{
	uint16 u16new_value;
	uint8 u8status = 0x01;
	uint8 u8counter;

	u8csurfaceCalibrationBackup[0] = (uint8)(u16aileron_calibrated_min >> 8);	
	u8csurfaceCalibrationBackup[1] = (uint8)(u16aileron_calibrated_min & 0xFF);
	
	u8csurfaceCalibrationBackup[2] = (uint8)(u16aileron_calibrated_center >> 8);	
	u8csurfaceCalibrationBackup[3] = (uint8)(u16aileron_calibrated_center & 0xFF);
	
	u8csurfaceCalibrationBackup[4] = (uint8)(u16aileron_calibrated_max >> 8);	
	u8csurfaceCalibrationBackup[5] = (uint8)(u16aileron_calibrated_max & 0xFF);
	
	u8csurfaceCalibrationBackup[6] = (uint8)(u16rudder_calibrated_min >> 8);	
	u8csurfaceCalibrationBackup[7] = (uint8)(u16rudder_calibrated_min & 0xFF);
	
	u8csurfaceCalibrationBackup[8] = (uint8)(u16rudder_calibrated_center >> 8);	
	u8csurfaceCalibrationBackup[9] = (uint8)(u16rudder_calibrated_center & 0xFF);
	
	u8csurfaceCalibrationBackup[10] = (uint8)(u16rudder_calibrated_max >> 8);	
	u8csurfaceCalibrationBackup[11] = (uint8)(u16rudder_calibrated_max & 0xFF);
	
	u8csurfaceCalibrationBackup[12] = (uint8)(u16elevator_calibrated_min >> 8);	
	u8csurfaceCalibrationBackup[13] = (uint8)(u16elevator_calibrated_min & 0xFF);
	
	u8csurfaceCalibrationBackup[14] = (uint8)(u16elevator_calibrated_center >> 8);	
	u8csurfaceCalibrationBackup[15] = (uint8)(u16elevator_calibrated_center & 0xFF);
	
	u8csurfaceCalibrationBackup[16] = (uint8)(u16elevator_calibrated_max >> 8);	
	u8csurfaceCalibrationBackup[17] = (uint8)(u16elevator_calibrated_max & 0xFF);	
	
	if (u8selection == CALIBRATE_AILERON_MIN)
	{
		u16new_value = u16aileron_temp;
		u8csurfaceCalibrationBackup[4] = (uint8)(u16new_value >> 8);
		u8csurfaceCalibrationBackup[5] = (uint8)(u16new_value & 0xFF);
	}
	else if (u8selection == CALIBRATE_AILERON_CENTER)
	{
		u16new_value = u16aileron_temp;
		u8csurfaceCalibrationBackup[2] = (uint8)(u16new_value >> 8);
		u8csurfaceCalibrationBackup[3] = (uint8)(u16new_value & 0xFF);
	}
	else if (u8selection == CALIBRATE_AILERON_MAX)
	{
		u16new_value = u16aileron_temp;
		u8csurfaceCalibrationBackup[0] = (uint8)(u16new_value >> 8);
		u8csurfaceCalibrationBackup[1] = (uint8)(u16new_value & 0xFF);
	}
	else if (u8selection == CALIBRATE_RUDDER_MIN)
	{
		u16new_value = u16rudder_temp;
		u8csurfaceCalibrationBackup[6] = (uint8)(u16new_value >> 8);
		u8csurfaceCalibrationBackup[7] = (uint8)(u16new_value & 0xFF);
	}
	else if (u8selection == CALIBRATE_RUDDER_CENTER)
	{
		u16new_value = u16rudder_temp;
		u8csurfaceCalibrationBackup[8] = (uint8)(u16new_value >> 8);
		u8csurfaceCalibrationBackup[9] = (uint8)(u16new_value & 0xFF);
	}
	else if (u8selection == CALIBRATE_RUDDER_MAX)
	{
		u16new_value = u16rudder_temp;
		u8csurfaceCalibrationBackup[10] = (uint8)(u16new_value >> 8);
		u8csurfaceCalibrationBackup[11] = (uint8)(u16new_value & 0xFF);
	}
	else if (u8selection == CALIBRATE_ELEVATOR_MIN)
	{
		u16new_value = u16elevator_temp;
		u8csurfaceCalibrationBackup[16] = (uint8)(u16new_value >> 8);
		u8csurfaceCalibrationBackup[17] = (uint8)(u16new_value & 0xFF);
	}
	else if (u8selection == CALIBRATE_ELEVATOR_CENTER)
	{
		u16new_value = u16elevator_temp;
		u8csurfaceCalibrationBackup[14] = (uint8)(u16new_value >> 8);
		u8csurfaceCalibrationBackup[15] = (uint8)(u16new_value & 0xFF);
	}
	else if (u8selection == CALIBRATE_ELEVATOR_MAX)
	{
		u16new_value = u16elevator_temp;
		u8csurfaceCalibrationBackup[12] = (uint8)(u16new_value >> 8);
		u8csurfaceCalibrationBackup[13] = (uint8)(u16new_value & 0xFF);
	}
	
	DisableInterrupts
	
	u8status &= u8FlashErase(0x4000);
	
	for (u8counter = 0; u8counter < 18; u8counter++)
	{
		u8status &= u8FlashProgramByte(0x4000 + u8counter, u8csurfaceCalibrationBackup[u8counter]);
	}
	
	EnableInterrupts
	
	return u8status;
}

uint8 u8Csurface_RestoreConfig (void)
{
	uint8 u8status = 0x01;
	uint8 u8counter;

	DisableInterrupts
	
	u8status &= u8FlashErase(0x4000);
	
	for (u8counter = 0; u8counter < 18; u8counter += 6 )
	{
		u8status &= u8FlashProgramByte(0x4000 + u8counter, (uint8)(CSURFACE_MIN_VALUE >> 8));
		u8status &= u8FlashProgramByte(0x4001 + u8counter, (uint8)(CSURFACE_MIN_VALUE & 0xFF)); 
	}
	
	for (u8counter = 2; u8counter < 18; u8counter += 6 )
	{
		u8status &= u8FlashProgramByte(0x4000 + u8counter, (uint8)(CSURFACE_CENTER_VALUE >> 8));
		u8status &= u8FlashProgramByte(0x4001 + u8counter, (uint8)(CSURFACE_CENTER_VALUE & 0xFF)); 
	}
	
	for (u8counter = 4; u8counter < 18; u8counter += 6 )
	{
		u8status &= u8FlashProgramByte(0x4000 + u8counter, (uint8)(CSURFACE_MAX_VALUE >> 8));
		u8status &= u8FlashProgramByte(0x4001 + u8counter, (uint8)(CSURFACE_MAX_VALUE & 0xFF)); 
	}
	
	EnableInterrupts;
	
	return u8status;
}

void vfnCsurface_CalibrateMinMax (void)
{
    if (u8surface_config == SURFACE_RESTORE_CONFIG_REQ) // If restore to default config is requested
    {
    	u8surface_config = 0x00;
    	
    	if(u8Csurface_RestoreConfig())
    	{
    		u8config_feedback3 |= (SURFACE_FEEDBACK_SUCCESS | 0x3F);
    	}
    	else
    	{
    		u8config_feedback3 |= (SURFACE_FEEDBACK_FAILURE | 0x3F);
    	}
    }
    else if (u8surface_config & SURFACE_AILERON_MIN_REQ)
    {
    	u8surface_config = 0x00;
    	
    	if(u8Csurface_SaveCalValue(CALIBRATE_AILERON_MIN))  // left
    	{
    		u8config_feedback3 |= (SURFACE_FEEDBACK_SUCCESS | SURFACE_AILERON_MIN_REQ);	
    	}
    	else
    	{
    		u8config_feedback3 |= (SURFACE_FEEDBACK_FAILURE | SURFACE_AILERON_MIN_REQ);
    	}
    }
    else if (u8surface_config & SURFACE_AILERON_MAX_REQ)
    {
    	u8surface_config = 0x00;
    	
    	if(u8Csurface_SaveCalValue(CALIBRATE_AILERON_MAX))  // right
    	{
    		u8config_feedback3 |= (SURFACE_FEEDBACK_SUCCESS | SURFACE_AILERON_MAX_REQ);	
    	}
    	else
    	{
    		u8config_feedback3 |= (SURFACE_FEEDBACK_FAILURE | SURFACE_AILERON_MAX_REQ);
    	}	
    }
    else if (u8surface_config & SURFACE_RUDDER_MIN_REQ)
    {
    	u8surface_config = 0x00;
    	
    	if(u8Csurface_SaveCalValue(CALIBRATE_RUDDER_MIN))  // left
    	{
    		u8config_feedback3 |= (SURFACE_FEEDBACK_SUCCESS | SURFACE_RUDDER_MIN_REQ);		
    	}
    	else
    	{
    		u8config_feedback3 |= (SURFACE_FEEDBACK_FAILURE | SURFACE_RUDDER_MIN_REQ);
    	}	
    }
    else if (u8surface_config & SURFACE_RUDDER_MAX_REQ)
    {
    	u8surface_config = 0x00;
    	
    	if(u8Csurface_SaveCalValue(CALIBRATE_RUDDER_MAX))  // right
    	{
    		u8config_feedback3 |= (SURFACE_FEEDBACK_SUCCESS | SURFACE_RUDDER_MAX_REQ);	
    	}
    	else
    	{
    		u8config_feedback3 |= (SURFACE_FEEDBACK_FAILURE | SURFACE_RUDDER_MAX_REQ);
    	}	
    }
    else if (u8surface_config & SURFACE_ELEVATOR_MIN_REQ)
    {
    	u8surface_config = 0x00;
    	
    	if(u8Csurface_SaveCalValue(CALIBRATE_ELEVATOR_MIN))  // up
    	{
    		u8config_feedback3 |= (SURFACE_FEEDBACK_SUCCESS | SURFACE_ELEVATOR_MIN_REQ);	
    	}
    	else
    	{
    		u8config_feedback3 |= (SURFACE_FEEDBACK_FAILURE | SURFACE_ELEVATOR_MIN_REQ);
    	}	
    }
    else if (u8surface_config & SURFACE_ELEVATOR_MAX_REQ)
    {
    	u8surface_config = 0x00;
    	
    	if(u8Csurface_SaveCalValue(CALIBRATE_ELEVATOR_MAX))  // down
    	{
    		u8config_feedback3 |= (SURFACE_FEEDBACK_SUCCESS | SURFACE_ELEVATOR_MAX_REQ);	
    	}
    	else
    	{
    		u8config_feedback3 |= (SURFACE_FEEDBACK_FAILURE | SURFACE_ELEVATOR_MAX_REQ);
    	}	
    }    
}









